![]() Procede pour la commande d'un robot
专利摘要:
公开号:WO1991003780A1 申请号:PCT/JP1990/001130 申请日:1990-09-04 公开日:1991-03-21 发明作者:Nobutoshi Torii;Ryo Nihei;Tetsuaki Kato 申请人:Fanuc Ltd; IPC主号:B25J9-00
专利说明:
[0001] 明 細 書 [0002] ロボ ッ ト制御方法 [0003] 技 術 分 野 [0004] 本発明は、 ロ ボ ッ ト制御方法に関し、 特に、 ロボッ ト のパネ要素の作用及びイ ナー シャ変動に起因する振動を 防止でき る ロボッ ト制御方法に関する。 [0005] 背 景 技 術 [0006] ロ ボ ッ ト のサ一 ボ系は、 典型的には、 サー ボモー タ に 装着した位置又は速度検出器からのフ ィ ー ドバッ ク信号 に従って線形制御たとえば比例積分制御を行う よ う にさ れ、 これによ りサーボモー タを駆動制御してロボッ トの 位置, 姿勢を制御する。 この種の従来のサー ボ系では口 ボ ッ ト の姿勢変化などに伴う イナー シャ変動は考慮され ず、 大き いイ ナ— シャ変動が生じる とサーボ系の応答特 性が変化する。 又、 ロボ ッ ト機構部は、 典型的には、 片 持ち梁状に構成される と共にパネ要素例えばロボッ ト機 構部とサ ー ボモー タ間に介在する減速機を含み、 従って、 その剛性及び共振周波数が低い。 その一方で、 従来のサ - ボ系ではパネ要素の作用は考慮されず、 ロ ボッ ト機構 部先端 ( ツールセ ンタ ポイ ン ト) の位置決め時に機構部 先端に振動が生じ易い。 そ こで、 従来は、 サーボゲイ ン を小さい値に設定し、 或は、 位置決め制御終了時点から 所定時間経過後に 口ボッ ト作業たとえばスポ ッ ト溶接を 開始して、 ロ ボ ッ ト機構部の振動に起因する作業精度低 下を防止 している。 従って、 従来法によれば、 所要位置 決め時間ひいてはロボッ ト作業のサイ ク ルタ ィ ムが増大 する不都合がある。 [0007] 発 明 の 開 示 [0008] 本発明の目的は、 ロボッ 卜に生じる振動をス ライ ディ ングモー ド制御によ り防止してロボ ッ ト作業を迅速に行 える ロ ボ ッ ト制御方法を提供する こ とにある。 [0009] 上述の目的を達成するため、 ロ ボ ッ ト機構部を駆動す るためのサ―ボモータを有するロボ ッ 卜に適用される本 発明のロ ボ ッ ト制御方法は、 サーボモータ回転位置及び 速度の夫々 に対する ロボッ ト機構部先端のね じれ量及び ね じれ速度を夫々検出する工程 ( a ) と、 ス ラ イ ディ ン グモー ド制御処理を行ってねじれ量及びね じれ速度を零 にする に足る制御出力を算出する工程 ( b ) と、 制御出 力に従っ てサーボモータを駆動する工程 ( c ) とを備え る。 [0010] 上述のよ う に、 本発明によれば、 ロボッ ト機構部先端 のねじれ量及びね じれ速度を零にするに足る ス ラ イ ディ ングモー ド制御出力に従ってサーボモータを駆動するよ う に したので、 ロ ボッ ト機構部に生じるね じれが迅速に 補償される。 こ のため、 ロ ボ ッ ト の姿勢変化などに伴つ てロボ ッ 卜 のイ ナ 一 シャが大き く 変動した場合にもサー ボ制御上の応答性が不適合になる こ とがな く、 口ボッ ト 機構部先端に生じ る振動が防止され、 或は、 速やかに減 衰する。 従って、 ロボ ッ ト機構部の位置決め制御完了後 直ちに ロ ボ ッ ト作業を開始でき、 所要位置決め時間ひい てはロ ボ ッ ト作業上の所要サイ ク ルタ イ ムを短縮でき る。 図 面 の 簡 単 な 説 明 [0011] 第 1 図は本発明の一実施例の制御方法が適用される 口 ボッ ト機構部を示す概略図、 [0012] 第 2 図はねじれ量及びね じれ速度を推定するためのォ ブザーバを構成するモデルを表すプロ ッ ク線図、 第 3 図はロボッ ト機構部を駆動制御するためのデジ夕 ルサー ボ回路を示す概略ブロ ッ ク図、 および 第 4 図はサーボ回路によ り実行される ト ルク指令算出 処理を示すフ ロ ー チ ャ ー ト である。 発明を実施するための最良の形態 [0013] 第 1 図を参照する と、 本発明の一実施例の制御方法が 適用される ロ ボ ッ 卜 の機構部は、 減速機 3 を介してサ— ボモータ 1 の出力軸に駆動的に連結したロ ボ ッ ト アー ム 4 を備え、 モータ 出力軸には速度検出器 2 が連結されて いる。 図示を簡略にするため、 ロボ ッ トアー ムの一つの 関節に関連する要素のみを第 1 図に示す。 減速機 3 は口 ボ ッ ト機構部のバネ要素及びダンバ要素と して機能し、 減速機 3 の回転時にその回転軸にね じれが生じる と共に 回転軸の軸受け部に摩擦が生じる。 以下、 減速機 3 のバ ネ定数及び粘性摩擦係数を記号 K c , B kで表す。 [0014] 次に、 減速機 3 に発生するねじれを捕償してロボッ ト アーム 4先端の振動を防止する本実施例方法の原理を説 明する。 [0015] 先ず、 減速機 3 の入力側及び出力側 (負荷側) の夫々 における ト ルク の釣合いから第 ( 1 ) 式及び第 ( 2 ) 式 に示す運動方程式を得る。 [0016] T = J m 0 m + B k ( Θ m - Θ r ) + K c { θ - θ r ) [0017] - - - ( 1 ) [0018] 0 = J τ Θ r + B τ ( θ τ - Θ m + Κ c ( θ r - θ m ) [0019] • · · ( 2 ) こ こで、 記号 Τ及び J mはサーボモータ 1 の入力 ト ルク および軸イ ナー シ ャ ( 口 — タ イ ナー シ ャ ) を夫々表し、 J r はモ―タ軸に換算した負荷イ ナー シャを表す。 記号 Θ m, 0 m及び 0 mは減速機入力側での減速機 3 の回転 位置, 速度及び加速度を夫々表し、 0 Γ, Θ r 及び Θ r は減速機入力側に換算した減速機出力側 (負荷側) での 減速機の回転位置, 速度及び加速度を夫々表す。 [0020] 第 ( 1 ) 式及び第 ( 2 ) 式から、 [0021] T " , B k B k、 , ,K c K c、 [0022] -— = ε + ( τ— + τ— ) ε + ( T— + ― ) ε J m J m J r J m J r [0023] · ,( 3 ) を得る。 こ こで、 ε ( = 0 m— 0 r ) 及び ( = m— Θ r ) は、 減速機 3 の入力側 (サーボモータ 1 ) の回転 位置及び速度の夫々に対する減速機出力側 (負荷〉 のね じれ量及びね じれ速度を夫々表す。 [0024] 次に、 第 ( 4 ) 式及び第 ( 5 ) 式に示すよ う に、 ね じ れ量 s とねじれ速度 の関数で表されるス ラ イ ディ ング モー ドでの切換え面 s と こ れに関連する リ ァプノ フ関数 ¾r ¾んる。 [0025] s = ε + C ε · · · ( 4 ) 一 - [0026] V s 2 ( 5 ) 第 ( 4 ) 式の記号 Cは実験的に設定されるパラ メ ータ を表す。 第 ( 5 ) 式の リ アプノ フ関数 V (≥ 0 ) はその 微分 Vが負であれば、 一様に減少して 0 に収束し、 換言 すれば、 切換え面 s ひいてはねじれ量 ε及びねじれ速度 s が 0 に夫々収束する。 以下、 そのための条件を求める ベく、 先ず、 Vを求める。 [0027] 第 ( 5 ) 式及び第 ( 4 ) 式の夫々 の両辺を微分して [0028] V = s s · · · ( 6 ) s = V + C ε · · · ( 7 ) を得る。 更に、 第 ( 3 ) 式及び第 ( 7 ) 式か ら第 ( 8 ) 式を、 第 ( 8 ) 式及び第 ( 4 ) 式から第 ( 9 ) 式を得る , [0029] T B k B k [0030] = ― + ( C一 [0031] j m [0032] K c c [0033] ) 5 [0034] m [0035] ( 8 ) c 2 B k ω _c 2 o [0036] T H- i C + ε ― J m C K c C [0037] ( 9 ) 第 ( 9 ) 式を第 ( 6 ) 式に代入して [0038] V [0039] を得 [0040] [0041] の平方根を表す。 [0042] ω c 2 s 2 [0043] 第 ( 1 0 ) 式の第 1 項 一 は常に負であり、 [0044] C [0045] 従って、 第 ( 1 1 ) 式或は これを変形して得た第 ( 1 2 ) 式が成立する と き V < 0 が成立する。 [0046] [0047] 方、 s < 0 である と き は第 ( 1 5〉 式が成立する と、 第 ( 1 1 ) 式及び第 ( 1 2 ) 式が成立する。 [0048] ― m [0049] T = - ( C 2 + ω c max2) ε [0050] し [0051] • • • ( 1 5 ) 以上のよう に、 s ≥ 0 のと きは第 ( 1 4 ) 式から、 又、 s < 0 のと きには第 ( 1 5 ) 式から V < 0 を満たす補正 トルク でが求められる。 [0052] そ して、 本実施例方法は、 通常の速度ルー プとス ライ ディ ン グモー ド制御を実行するためのマイ ナ ールー プと を備えたサーボ系によ り実施され、 通常の速度ループで 得た通常の ト ルク指令にマ イ ナ — ループで得た補正 ト ル ク を加算 して ト ルク指令を得る。 こ の ト ルク指令に従う サーボモ ー タ の運転時、 切換え面 s が 0 に収束する。 こ のと き、 ねじれ量 ε及びね じれ速度 ε が夫々 0 に収束し、 換言すれば、 ロ ボ ッ ト機構部先端の振動が防止され或は [0053] B J一 [0054] 速やかに減衰_ m kする。 又、 第 ( 1 ) 式から明かなよ う に、 ε = ε = 0 が成立 B Jす一るのでサ―ボモータ出力 ト ルク は時 間的に不変の口 — タ一 k rイ ナ— シ ャ J mのみに依存し、 換言 すれば、 イ ナー シ ャ変動が補償される。 [0055] 本実施例では、 ねじれ量 ε及びね じれ速度 ε の検出に、 [0056] K J一 [0057] 両パラ メ 一 夕 を推定するためのオ一 m Cブザーバを用いる。 ォ ブザーバを構成するモデルを決定するため、 第 i 図の口 [0058] K J- ボ ッ ト機構部に関する状態方程式を求め一 C Γる。 [0059] 先ず、 第 ( 3 ) 式及び第 ( 1 ) 式から第 ( 1 6 ) 式及 び第 ( 1 7 ) 式を夫々得る。 [0060] T [0061] ε + [0062] m 6 ) [0063] Β k Κ c Τ [0064] θ m ε ― ε + [0065] m J m J m t Λ · · · ( 1 7 〉 こ こ で = x l, ε = χ 2および 0 m = x 3とおく と、 第 ( 1 6 ) 式及び第 ( 1 7 ) 式から第 ( 1 8 ) 式及び第 [0066] ( 2 0 ) 式が夫々得られ、 又、 第 ( 1 9 ) 式が成立する < [0067] * , , B k , B k , , , , Κ ο , Κ ε ヽ 。 x 1= - ( Τ" + —一 ) x l+ ( τ一 + τ— ) X 2 [0068] J m J r J m J r [0069] T [0070] + ~r— · · · ( l 8 ) [0071] J m [0072] x 2 = x 1 - - - ( 1 9 ) [0073] , 。 B k K c 0 , T [0074] x 3 = 一 . ~ 1 - 一- x 2 + [0075] m J m J m ( 2 0 ) [0076] ( 1 8 ) 式〜第 ( 2 0 ) 式から [0077] -{Bk/Jm)†(Bk/Jr)} - {Kc/Jra)+( c/Jr) [0078] [ B 'k/Jm - c/Jin [0079] ( 2 1 ) を得る。 更に、 第 ( 2 1 ) 式を簡潔に表現する こ と によ り、 第 ( 2 2 ) 式に示す状態方程式を得る。 d X [0080] = Ax - K D x + K D x + B T [0081] d t [0082] (A— K D) x +K y + B T ( 2 2 ) [0083] -X ' x で、 x = [ ] , y = C O, o , 。 ] [^] であり、 A, B, Dは行列を表し、 Kは A— K Dが安定 するよ う に選択した定数を表す。 [0084] 第 ( 2 2 ) 式に照ら してオブザー バを構成するモデル は第 ( 2 3 ) 式で表され、 換言すれば、 第 2図のプ XXXロ ッ ク線図で表される。 [0085] A [0086] X = (A— K D) x + K y + B T ( 2 3 ) d t [0087] こ こで、 x ( = 「¾H] ) は Xの推定値を表し、 下記の 関係を満たす。 [0088] すなわち、 第 2図のォブザ バによ り、 ね じれ量 ε及び ねじれ速度 を推定でき る [0089] 以下、 第 3図を参照して、 上記ス ラ イ ディ ングモー ド 制御方法を実施するためのデジタ ルサーポ (ソ フ ト ウ ヱ アサーボ) 系を説明する。 [0090] デジ夕 ルサー ボ系はデジタ ルシグナルプロ セ ッサ (図 示略) 及び後述の各種定数の設定値を格納するためのメ モ リ (図示略) を内蔵したデジタ ルサーボ回路 1 3 を備 え、 こ のサーポ回路 1 3 は、 第 1 図のロボ ッ ト機構部を 備える ロ ボッ ト 1 5 の各軸サーボモータ (第 1 図のサー ボモー タ 1 に夫々対応) の位置, 速度及び電流の制御及 びス ラ イ ディ ングモ— ド制御な らびにオブザーバの機能 をソ フ ト ゥ ヱァ処理によ り実行又は達成する よ う にされ ている。 [0091] また、 サーボ系は、 サー ボ回路 1 3 のプロ セ ッサ (以 下、 第 1 プロセ ッ サという〉 及び移動指令を分配するた めの数値制御装置等のホ ス ト コ ン ピュ ー タ 1 1 のプロセ ッサ (以下、 第 2 プロセ ッ サという) の双方からァクセ ス可能に設けた共有メ モ リ ( R A M ) 1 2 と、 サーボモ 一夕 に流れる実際駆動電流を検出するための検出器 (図 示略) と、 サ―ボ回路 1 3 からの電流指令及び電流検出 器出力に応動して各軸サー ボモータ を駆動するためのサ ー ボア ンプ (図示略) とを備えている。 更に、 サーポ系 は、 夫々 のサーボモータ に装着した速度検出器 (第 1 図 の検出器 2 に対応) と、 実際モータ回転位置を表す速度 検出器か ら送出されるフ ィ ー ドバッ クパルス数を記憶す る と共に実際モー タ電流を表す電流検出器出力を記憶す るための帰還信号レ ジス タ 1 4 と、 各種定数を入力する ための手動データ入力装置 (図示略) とを備えている。 以下、 第 4図を参照して、 サーボ系の ト ルク指令算出 処理を説明する。 ロ ボ ッ ト運転前に、 ス ラ イ ディ ン グモー ド制御および オブザー バ処理に要する各種定数 (パラ メ ー タ C, サー ボモー タ の軸イ ナ 一 シ ャ J m, ノ、。ラ メ 一夕 ω c の最大値 ω c max, 行列 A, B , D ) が手動データ入力装置を介 して入力される と、 第 1 プロセ ッ サの制御下でサーポ回 路 1 3 の メ モ リ に格納される。 [0092] ロボ ッ ト運転中、 第 1 プロセ ッ サは、 第 2 プロセ ッサ による移動指令分配周期と同一周期で第 4 図の処理を実 行する。 [0093] 即ち、 第 1 プロセ ッサは、 各々の制御周期において、 先ず、 第 2 プロセ ッサから共有 R A M I 2 に書き込まれ た指令モータ回転位置を読み取る と共に (第 4図のステ ッ プ 1 0 0 ) 、 帰還信号レ ジスタ 1 4から実際サーボモ — タ回転位置 (位置フ ィ — ドバッ ク値) を読取り (ステ ッ プ 1 0 1 ) 、 従来公知の通常の位置ループ処理及び速 度ループ処理を実行する ( ステ ッ プ 1 0 2 ) 。 次に、 第 1 プロセ ッ サは位置ループ処理で算出した速度指令 (第 2図の 0 m) 及び前回制御周期での速度ルー プ処理で算 出され当該プロセ ッサに内蔵のレ ジスタ (図示略) に記 憶した ト ルク指令値 (第 2 図の T ) に基づいて第 ( 2 3 ) 式および第 2図に対応するオブザーバ処理を実行してね じれ量 s 及びね じれ速度 ε を夫々算出し (ス テ ッ プ 1 0 3 ) 、 これら算出値 ε , ^ を第 ( 4 ) 式に代入して切換 え面 s を求める (ステ ッ プ 1 0 4 ) 。 そ して、 切換え面 s の正負に応じて第 ( 1 4 ) 式及び第 ( 1 5 ) 式の対応 する もの に従って補正 ト ルク Tを算出する ( ステ ッ プ 1 0 6, 1 0 7 ) 。 次いで、 第 1 プロ セ ッ サは、 速度ルー プ処理で算出した ト ルク指令値にス テ ッ プ 1 0 6又は 1 0 7 で算出した補正 ト ルク Τを加算して補正 ト ルク指令 値 (電流指令値) を求め、 この補正 ト ルク指令値に基づ いて電流ループ処理を実行する (ス テ ッ プ 1 0 8 ) 。 [0094] 電流ループ処理の実行中、 サーボアンプは電流指令値 及び実際電流値を表す電流検出器出力に応じて各軸サー ボモ— 夕 を駆動制御する。 このと き、 上述のよう に、 ね じれ量 ε 及びね じれ速度 が零になるよ う に各軸サーボ モー タ の出力 ト ルク が制御される。 結果と して、 ロボッ ト の姿勢変化に伴ってロボ ッ トのイ ナー シ ャが変動した と して も、 一定の応答関数 ( s = 0 ) で表されるサーボ 系の応答性は変化しない。 従って、 ロボッ ト機構部先端 の振動が防止され或は速やかに減衰する。 こ のため、 口 ボ ッ ト機構部先端の位置決め動作が短時間内で完了し、 ロ ボ ッ ト作業上のサイ ク ルタ イ ムを短縮でき る。 [0095] 本発明は上記実施例に限定されず、 種々の変形が可能 [0096] C、め 0 [0097] 例えば、 上記実施例ではねじれ量 s及びね じれ速度 の検出にオブザー バを用いたが、 これに代えて、 減速機 3 の出力側に第 2 速度検出器を設けても良い。 この場合、 減速機の入力側に設けた第 1 図の速度検出器 2 からの減 速機入力側での減速機回転位置 e m及び速度 0 mを表す 出力と第 2速度検出器からの減速機出力側での減速機回 転位置 0 r及び速度 r を表す出力とに基づいてねじれ 量 ε及びねじれ速度 s を検出する。
权利要求:
Claims請 求 の 範 囲 1 . ロ ボ ッ ト機構部を駆動するためのサ― ボモータ を有 する ロ ボッ 卜 に適用される ロボ ッ ト制御方法において、 前記サーボモー タの回転位置及び速度の夫々 に対する 前記ロ ボッ ト機構部の先端のね じれ量及びねじれ速度 を夫々検出する工程 ( a ) と、 ス ラ イ ディ ングモー ド 制御処理を行って前記ね じれ量及び前記ね じれ速度を 零にするに足る制御出力を算出する工程 ( b ) と、 前 記制御出力に従って前記サーボモータを駆動する工程 ( c ) とを備える こ とを特徴とする ロボ ッ ト制御方法。 2 . 前記工程 ( b ) は、 速度ループ処理を行って ト ルク 指令を算出する工程 ( b l ) と、 前記ス ラ イ ディ ング モー ド制御処理を行って前記ね じれ量及び前記ねじれ 速度を零にするための補正 ト ルク を算出する工程 ( b 2 ) と、 前記 ト ルク指令に前記捕正 ト ルク を加算して 前記制御出力と しての補正 ト ルク指令を得る工程 ( b 3 ) とを含む請求の範囲第 1 項記載のロ ボ ッ ト制御方 法。 3 . 前記工程 ( c ) は、 前記補正 ト ルク指令に基づく電 流ルー プ処理を含む請求の範囲第 2項記載のロボッ ト 制御方法。 4 . 前記工程 ( a ) において、 前記ロ ボ ッ ト機構部を表 す状態方程式に基づいてモデル化したォブザーバを用 いて前記ねじれ量及び前記ねじれ速度を推定する請求 の範囲第 1 項記載のロ ボ ッ ト制御方法。 前記サーボモータ と前記口ポ ッ ト機構部間に介在し かつ回転時にね じれを発生する減速機と、 前記減速機 の入力側での前記減速機の回転位置及び速度を表す第 1 出力を発生するための第 1検出器と、 前記減速機の 出力側での前記減速機の回転位置及び速度を表す第 2 出力を発生するための第 2検出器とを有するロボッ ト に適用され、 前記工程 ( a ) において、 前記第 1 出力 及び前記第 2 出力に基づいて前記ねじれ量及び前記ね じれ速度を検出する請求の範囲第 1項記載のロボッ ト 制御方法。
类似技术:
公开号 | 公开日 | 专利标题 CN102189550B|2014-06-04|具有学习控制功能的机器人 JP5214578B2|2013-06-19|動力装置の制御装置 US6784632B2|2004-08-31|Positioning servo controller JP4283214B2|2009-06-24|機械先端点の制御装置 JP2802267B2|1998-09-24|システムの入力出力トルク・ゲイン線形化による電気アシスト・ステアリング・システム制御方法及び装置 US5157597A|1992-10-20|Method of detecting oscillation of a servo system and automatically adjusting speed loop gain thereof US6826450B2|2004-11-30|Robot controller US5049796A|1991-09-17|Robust high-performance control for robotic manipulators EP1132790B1|2003-06-18|Controller for machine CN100396452C|2008-06-25|加减速控制方法及装置 CN100524106C|2009-08-05|电动机控制装置的自动调整法及装置 JP3215067B2|2001-10-02|移動方向反転時の位置補正方法 US4989161A|1991-01-29|Control unit for a multi-degree-of freedom manipulator US4663726A|1987-05-05|Robot control utilizing cubic spline interpolation JP2762364B2|1998-06-04|サーボモータのフィードフォワード制御方法 US9013131B2|2015-04-21|Real-time servo motor controller based on a load weight EP0426873B1|1996-03-27|Sliding mode control method with feedforward compensation function US20020087241A1|2002-07-04|System of informing procedures for adjusting control parameters of an electric power steering control apparatus EP0289150B1|1992-06-17|Motion control method with minimum time path generation Teeter et al.1996|A novel fuzzy friction compensation approach to improve the performance of a DC motor control system EP0113010A1|1984-07-11|Robotersteuerung US20100228440A1|2010-09-09|Steering control apparatus and a steering apparatus using the same US20020026267A1|2002-02-28|Electric power steering system EP0289151B1|1994-01-12|Motion control apparatus having adaptive feedforward path tracking US20090200978A1|2009-08-13|Robot controller having component protecting function and robot control method
同族专利:
公开号 | 公开日 JPH0392911A|1991-04-18| EP0441983A4|1991-08-28| EP0441983A1|1991-08-21|
引用文献:
公开号 | 申请日 | 公开日 | 申请人 | 专利标题
法律状态:
1991-03-21| AK| Designated states|Kind code of ref document: A1 Designated state(s): US | 1991-03-21| AL| Designated countries for regional patents|Kind code of ref document: A1 Designated state(s): AT BE CH DE DK ES FR GB IT LU NL SE | 1991-05-02| WWE| Wipo information: entry into national phase|Ref document number: 1990912964 Country of ref document: EP | 1991-08-21| WWP| Wipo information: published in national office|Ref document number: 1990912964 Country of ref document: EP | 1992-08-28| WWW| Wipo information: withdrawn in national office|Ref document number: 1990912964 Country of ref document: EP |
优先权:
[返回顶部]
申请号 | 申请日 | 专利标题 相关专利
Sulfonates, polymers, resist compositions and patterning process
Washing machine
Washing machine
Device for fixture finishing and tension adjusting of membrane
Structure for Equipping Band in a Plane Cathode Ray Tube
Process for preparation of 7 alpha-carboxyl 9, 11-epoxy steroids and intermediates useful therein an
国家/地区
|